
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       blog_interface.c
  * @author     baiyang
  * @date       2021-8-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdint.h>

#include <rtthread.h>

#include "blog.h"
#include "blog_interface.h"
#include <common/time/gp_time.h>
#include <uITC/uITC.h>
#include <common/console/console.h>
#include <uITC/uITC_msg.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static struct rt_mutex _lock;

static itc_node_t pNodeAttCtrl       = NULL;
static itc_node_t pNodePosXY         = NULL;
static itc_node_t pNodePosCtrlXY       = NULL;
static itc_node_t pNodePosCtrlZ       = NULL;
static itc_node_t pNodeRcRaw         = NULL;
static itc_node_t pNodeSRVOut        = NULL;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       初始化互斥锁
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
int32_t blog_init_mutex()
{
    return rt_mutex_init(&_lock, "blog", RT_IPC_FLAG_FIFO);
}

/**
  * @brief       以阻塞方式获取互斥锁
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
int32_t blog_take_mutex_block()
{
    return rt_mutex_take(&_lock, RT_WAITING_FOREVER);
}

/**
  * @brief       以非阻塞方式获取互斥锁
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
int32_t blog_take_mutex_noblock()
{
    return rt_mutex_take(&_lock, 0);
}

/**
  * @brief       释放互斥锁
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
int32_t blog_release_mutex()
{
    return rt_mutex_release(&_lock);
}

/**
  * @brief       记录IMU数据
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_imu(const float gyr[3],const float acc[3],const float filter_gyr[3],const float filter_acc[3])
{
    uint32_t NowMs = time_millis();

    blog_imu pkt = {
        LOG_PACKET_HEADER_INIT(BLOG_IMU_ID),
        .time_ms    = NowMs,
        .gyr_x      = Rad2Deg(gyr[0]),
        .gyr_y      = Rad2Deg(gyr[1]),
        .gyr_z      = Rad2Deg(gyr[2]),
        .acc_x      = acc[0],
        .acc_y      = acc[1],
        .acc_z      = acc[2],
        .fil_gyr_x  = Rad2Deg(filter_gyr[0]),
        .fil_gyr_y  = Rad2Deg(filter_gyr[1]),
        .fil_gyr_z  = Rad2Deg(filter_gyr[2]),
        .fil_acc_x  = filter_acc[0],
        .fil_acc_y  = filter_acc[1],
        .fil_acc_z  = filter_acc[2],
        LOG_PACKET_END_INIT,
    };

    blog_push_msg_data(&pkt, sizeof(pkt));
}

/**
  * @brief       记录IMU数据
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_imu2()
{
    uitc_sensor_gyr sensor_gyr = {0};
    uitc_sensor_acc sensor_acc = {0};

    if (!((itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc) == 0))) {
        return;
    }

    blog_imu pkt = {
        LOG_PACKET_HEADER_INIT(BLOG_IMU_ID),
        .time_ms    = sensor_gyr.timestamp_us*1e-3,
        .gyr_x      = Rad2Deg(sensor_gyr.sensor_gyr_correct[0]),
        .gyr_y      = Rad2Deg(sensor_gyr.sensor_gyr_correct[1]),
        .gyr_z      = Rad2Deg(sensor_gyr.sensor_gyr_correct[2]),
        .acc_x      = sensor_acc.sensor_acc_correct[0],
        .acc_y      = sensor_acc.sensor_acc_correct[1],
        .acc_z      = sensor_acc.sensor_acc_correct[2],
        .fil_gyr_x  = Rad2Deg(sensor_gyr.sensor_gyr_filter[0]),
        .fil_gyr_y  = Rad2Deg(sensor_gyr.sensor_gyr_filter[1]),
        .fil_gyr_z  = Rad2Deg(sensor_gyr.sensor_gyr_filter[2]),
        .fil_acc_x  = sensor_acc.sensor_acc_filter[0],
        .fil_acc_y  = sensor_acc.sensor_acc_filter[1],
        .fil_acc_z  = sensor_acc.sensor_acc_filter[2],
        LOG_PACKET_END_INIT,
    };

    blog_push_msg_data(&pkt, sizeof(pkt));
}

/**
  * @brief       记录磁罗盘数据
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_mag()
{
    uitc_sensor_mag sensor_mag;

    if (itc_copy_from_hub(ITC_ID(sensor_mag), &sensor_mag) == 0) {
        blog_mag pkt1 = {
            LOG_PACKET_HEADER_INIT(BLOG_MAG_ID),
            .time_ms    = sensor_mag.timestamp_us*1e-3,
            .mag_x      = sensor_mag.sensor_mag_correct[0],
            .mag_y      = sensor_mag.sensor_mag_correct[1],
            .mag_z      = sensor_mag.sensor_mag_correct[2],
            LOG_PACKET_END_INIT,
        };

        blog_mag pkt2 = {
            LOG_PACKET_HEADER_INIT(BLOG_MAG_FIL_ID),
            .time_ms    = sensor_mag.timestamp_us*1e-3,
            .mag_x      = sensor_mag.sensor_mag_filter[0],
            .mag_y      = sensor_mag.sensor_mag_filter[1],
            .mag_z      = sensor_mag.sensor_mag_filter[2],
            LOG_PACKET_END_INIT,
        };

        blog_mag pkt3 = {
            LOG_PACKET_HEADER_INIT(BLOG_MAG_MEA_ID),
            .time_ms    = sensor_mag.timestamp_us*1e-3,
            .mag_x      = sensor_mag.sensor_mag_measure[0],
            .mag_y      = sensor_mag.sensor_mag_measure[1],
            .mag_z      = sensor_mag.sensor_mag_measure[2],
            LOG_PACKET_END_INIT,
        };

        blog_push_msg_data(&pkt1, sizeof(pkt1));
        blog_push_msg_data(&pkt2, sizeof(pkt2));
        blog_push_msg_data(&pkt3, sizeof(pkt3));
    }
}

/**
  * @brief       记录gps
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_gps()
{
    static itc_node_t pNodeGps = NULL;

    uitc_sensor_gps vehicle_gps_position;

    if (pNodeGps == NULL) {
        pNodeGps = itc_subscribe(ITC_ID(sensor_gps), NULL);
        return;
    }

    if (itc_update(pNodeGps)) {
        itc_copy(ITC_ID(sensor_gps), pNodeGps, &vehicle_gps_position);

        blog_gps pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_GPS_ID),
            .time_ms    = vehicle_gps_position.timestamp_us*1e-3,
            .timegpsms  = vehicle_gps_position.time_gps_usec*0.001f,
            .fixType    = vehicle_gps_position.fix_type,
            .numSV      = vehicle_gps_position.num_sats,
            .lon        = vehicle_gps_position.lon,
            .lat        = vehicle_gps_position.lat,
            .height     = vehicle_gps_position.alt*1e-2,
            .hAcc       = vehicle_gps_position.horizontal_accuracy,
            .vAcc       = vehicle_gps_position.vertical_accuracy,
            .sAcc       = vehicle_gps_position.s_variance_m_s,
            .velN       = vehicle_gps_position.vel_n_m_s,
            .velE       = vehicle_gps_position.vel_e_m_s,
            .velD       = vehicle_gps_position.vel_d_m_s,
            .speed      = vehicle_gps_position.vel_m_s,
            .headMot    = vehicle_gps_position.cog_rad,
            .headAcc    = vehicle_gps_position.c_variance_rad,
            LOG_PACKET_END_INIT,
        };

        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录姿态数据
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note        三轴角度、三轴速度，单位：degrees,degrees/s
  */
void blog_write_att()
{
    uitc_vehicle_attitude vehicle_attitude;
    uitc_sensor_gyr sensor_gyr;

    if (itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude) == 0 && \
        itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr) == 0) {
        blog_att pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_ATT_ID),
            .time_ms    = sensor_gyr.timestamp_us*1e-3,
            .roll       = Rad2Deg(vehicle_attitude.vehicle_euler.roll),
            .pitch      = Rad2Deg(vehicle_attitude.vehicle_euler.pitch),
            .yaw        = Rad2Deg(vehicle_attitude.vehicle_euler.yaw),
            .RollRate   = Rad2Deg(sensor_gyr.sensor_gyr_filter[0]),
            .PitchRate  = Rad2Deg(sensor_gyr.sensor_gyr_filter[1]),
            .YawRate    = Rad2Deg(sensor_gyr.sensor_gyr_filter[2]),
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}


/**
  * @brief       记录姿态控制量数据
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note        目标三轴角度、目标三轴速度，单位：rad,rad/s
  */
void blog_write_att_ctrl()
{
    uitc_vehicle_att_ctrl att_ctrl;

    if (pNodeAttCtrl == NULL) {
        pNodeAttCtrl = itc_subscribe(ITC_ID(vehicle_att_ctrl), NULL);
    }

    if (pNodeAttCtrl == NULL) {
        return;
    }

    if (itc_update(pNodeAttCtrl)) {
        itc_copy(ITC_ID(vehicle_att_ctrl), pNodeAttCtrl, &att_ctrl);

        blog_atc pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_ATC_ID),
            .time_ms        = att_ctrl.timestamp_us*1e-3,
            .DesRoll        = Rad2Deg(att_ctrl.target_euler_angle[0]),
            .DesPitch       = Rad2Deg(att_ctrl.target_euler_angle[1]),
            .DesYaw         = Rad2Deg(att_ctrl.target_euler_angle[2]),
            .DesRollRate    = Rad2Deg(att_ctrl.target_ang_vel_body[0]),
            .DesPitchRate   = Rad2Deg(att_ctrl.target_ang_vel_body[1]),
            .DesYawRate     = Rad2Deg(att_ctrl.target_ang_vel_body[2]),
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录位置信息
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_pos_xy()
{
    uitc_vehicle_position vehicle_position;

    if (pNodePosXY == NULL) {
        pNodePosXY = itc_subscribe(ITC_ID(vehicle_position), NULL);
    }

    if (pNodePosXY == NULL) {
        return;
    }

    if (itc_update(pNodePosXY)) {
        itc_copy(ITC_ID(vehicle_position), pNodePosXY, &vehicle_position);

        blog_pos_xy pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_POS_XY_ID),
            .time_ms        = vehicle_position.timestamp_us*1e-3,
            .Px             = vehicle_position.x*100,
            .Py             = vehicle_position.y*100,
            .Vx             = vehicle_position.vx*100,
            .Vy             = vehicle_position.vy*100,
            .Ax             = vehicle_position.ax*100,
            .Ay             = vehicle_position.ay*100,
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}


/**
  * @brief       记录期望位置数据
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_pos_ctrl_xy()
{
    uitc_vehicle_pos_ctrl_xy vehicle_pos_ctrl_xy;

    if (pNodePosCtrlXY == NULL) {
        pNodePosCtrlXY = itc_subscribe(ITC_ID(vehicle_pos_ctrl_xy), NULL);
    }

    if (pNodePosCtrlXY == NULL) {
        return;
    }

    if (itc_update(pNodePosCtrlXY)) {
        itc_copy(ITC_ID(vehicle_pos_ctrl_xy), pNodePosCtrlXY, &vehicle_pos_ctrl_xy);

        blog_psc_xy pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_PSC_XY_ID),
            .time_ms           = vehicle_pos_ctrl_xy.timestamp_us*1e-3,
            .DesPx             = vehicle_pos_ctrl_xy.target_pos_xy[0],
            .DesPy             = vehicle_pos_ctrl_xy.target_pos_xy[1],
            .DesVx             = vehicle_pos_ctrl_xy.target_vel_xy[0],
            .DesVy             = vehicle_pos_ctrl_xy.target_vel_xy[1],
            .DesAx             = vehicle_pos_ctrl_xy.target_acc_xy[0],
            .DesAy             = vehicle_pos_ctrl_xy.target_acc_xy[1],
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录纵向位置信息
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_pos_z()
{
    uitc_vehicle_alt vehicle_alt;

    if (itc_copy_from_hub(ITC_ID(vehicle_alt), &vehicle_alt) == 0) {
        blog_pos_z pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_POS_Z_ID),
            .time_ms    = vehicle_alt.timestamp_us*1e-3,
            .Pz         = vehicle_alt.relative_alt*100,
            .Vz         = vehicle_alt.vz*100,
            .Az         = vehicle_alt.az*100,
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}


/**
  * @brief       记录垂直方向控制量
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_vertical_ctrl()
{
    uint32_t NowMs = time_millis();
    bool itc_res = 0;

    uitc_vehicle_att_ctrl att_ctrl = {0};
    uitc_sensor_baro baro = {0};
    uitc_vehicle_pos_ctrl_z pos_ctrl_z = {0};
    uitc_vehicle_alt vehicle_alt = {0};

    itc_res = (itc_copy_from_hub(ITC_ID(vehicle_att_ctrl), &att_ctrl) == 0);
    itc_res = (itc_copy_from_hub(ITC_ID(vehicle_pos_ctrl_z), &pos_ctrl_z) == 0) || itc_res;
    itc_res = (itc_copy_from_hub(ITC_ID(vehicle_alt), &vehicle_alt) == 0) || itc_res;
    itc_res = (itc_copy_from_hub(ITC_ID(sensor_baro), &baro) == 0) || itc_res;

    if (itc_res) {
        blog_ver_ctrl pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_VER_CTRL_ID),
            .time_ms           = NowMs,
            .ThrIn             = att_ctrl.throttle_in,
            .DesAlt            = pos_ctrl_z.target_pos_z,
            .InavAlt           = vehicle_alt.relative_alt*100,
            .BaroAlt           = baro.altitude_cm,
            .TargetClimbRate   = pos_ctrl_z.target_vel_z,
            .ClimbRate         = vehicle_alt.vz*100,
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录纵向期望位置
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_pos_ctrl_z()
{
    uitc_vehicle_pos_ctrl_z pos_ctrl_z;

    if (pNodePosCtrlZ == NULL) {
        pNodePosCtrlZ = itc_subscribe(ITC_ID(vehicle_pos_ctrl_z), NULL);
    }
    
    if (pNodePosCtrlZ == NULL) {
        return;
    }

    if (itc_update(pNodePosCtrlZ)) {
        itc_copy(ITC_ID(vehicle_pos_ctrl_z), pNodePosCtrlZ, &pos_ctrl_z);
        
        blog_psc_z pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_PSC_Z_ID),
            .time_ms           = pos_ctrl_z.timestamp_us*1e-3,
            .DesPz             = pos_ctrl_z.target_pos_z,
            .DesVz             = pos_ctrl_z.target_vel_z,
            .DesAz             = pos_ctrl_z.target_acc_z,
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录遥控器输入
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_rc_in()
{
    uitc_actuator_rc actuator_rc = {0};

    if (pNodeRcRaw == NULL) {
        pNodeRcRaw = itc_subscribe(ITC_ID(vehicle_rc), NULL);
    }

    if (pNodeRcRaw == NULL) {
        return;
    }

    if (itc_update(pNodeRcRaw)) {
        itc_copy(ITC_ID(vehicle_rc), pNodeRcRaw, &actuator_rc);

        blog_rc_in pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_RC_IN_ID),
            .time_ms           = actuator_rc.timestamp_us*1e-3,
            .chan1_raw         = actuator_rc.channels[0],
            .chan2_raw         = actuator_rc.channels[1],
            .chan3_raw         = actuator_rc.channels[2],
            .chan4_raw         = actuator_rc.channels[3],
            .chan5_raw         = actuator_rc.channels[4],
            .chan6_raw         = actuator_rc.channels[5],
            .chan7_raw         = actuator_rc.channels[6],
            .chan8_raw         = actuator_rc.channels[7],
            .chan9_raw         = actuator_rc.channels[8],
            .chan10_raw        = actuator_rc.channels[9],
            .chan11_raw        = actuator_rc.channels[10],
            .chan12_raw        = actuator_rc.channels[11],
            .chan13_raw        = actuator_rc.channels[12],
            .chan14_raw        = actuator_rc.channels[13],
            .chan15_raw        = actuator_rc.channels[14],
            .chan16_raw        = actuator_rc.channels[15],
            .chan17_raw        = actuator_rc.channels[16],
            .chan18_raw        = actuator_rc.channels[17],
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}



/**
  * @brief       记录PWM输出
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_srv_out()
{
    uitc_actuator_outputs srv_out = {0};      //数组长度尽量的大，防止，PWM输出增加时数组越界

    if (pNodeSRVOut == NULL) {
        pNodeSRVOut = itc_subscribe(ITC_ID(vehicle_actuator_outputs), NULL);
    }

    if (pNodeSRVOut == NULL) {
        return;
    }

    if (itc_update(pNodeSRVOut)) {
        itc_copy(ITC_ID(vehicle_actuator_outputs), pNodeSRVOut, &srv_out);

        blog_srv_out pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_SRV_OUT_ID),
            .time_ms           = srv_out.timestamp_us*1e-3,
            .servo1_raw        = srv_out.output[0],
            .servo2_raw        = srv_out.output[1],
            .servo3_raw        = srv_out.output[2],
            .servo4_raw        = srv_out.output[3],
            .servo5_raw        = srv_out.output[4],
            .servo6_raw        = srv_out.output[5],
            .servo7_raw        = srv_out.output[6],
            .servo8_raw        = srv_out.output[7],
            LOG_PACKET_END_INIT,
        };
        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录飞行模式
  * @param[in]   mode
  * @param[in]   mode_reason
  * @param[out]
  * @retval
  * @note
  */
void blog_write_mode(uint8_t mode, uint8_t mode_reason)
{
    uint32_t NowMs = time_millis();

    blog_flight_mode pkt = {
        LOG_PACKET_HEADER_INIT(BLOG_MODE_ID),
        .time_ms           = NowMs,
        .mode              = mode,
        .ModeReason        = mode_reason,
        LOG_PACKET_END_INIT,
    };

    blog_push_msg_data(&pkt, sizeof(pkt));
}

/**
  * @brief       记录一些事件
  * @param[in]   event
  * @param[in]   event_code
  * @param[out]
  * @retval
  * @note
  */
void blog_write_event(uint8_t event, int8_t event_code)
{
    uint32_t NowMs = time_millis();

    blog_event pkt = {
        LOG_PACKET_HEADER_INIT(BLOG_EVENT_ID),
        .time_ms           = NowMs,
        .event             = event,
        .EventCode         = event_code,
        LOG_PACKET_END_INIT,
    };

    blog_push_msg_data(&pkt, sizeof(pkt));
}

/**
  * @brief       记录姿态PID目标量和误差量
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void blog_write_att_pid()
{
    uitc_vehicle_att_ctrl vehicle_att_ctrl;

    if (itc_copy_from_hub(ITC_ID(vehicle_att_ctrl), &vehicle_att_ctrl) == 0) {
        blog_pid pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_PID_ID),
            .time_ms           = vehicle_att_ctrl.timestamp_us*1e-3,
            .RollRatTar        = Rad2Deg(vehicle_att_ctrl.ctrl_r_pid_target),
            .RollRatErr        = Rad2Deg(vehicle_att_ctrl.ctrl_r_pid_error),
            .PitchRatTar       = Rad2Deg(vehicle_att_ctrl.ctrl_p_pid_target),
            .PitchRatErr       = Rad2Deg(vehicle_att_ctrl.ctrl_p_pid_error),
            .YawRatTar         = Rad2Deg(vehicle_att_ctrl.ctrl_y_pid_target),
            .YawRatErr         = Rad2Deg(vehicle_att_ctrl.ctrl_y_pid_error),
            LOG_PACKET_END_INIT,
        };

        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

#if 0
/**
  * @brief       记录航点命令
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void Blog_WriteMissionCmd(const Mission_HandleTypeDef* mission, const Mission_Command* cmd)
{
    uint32_t NowMs = time_nowMs();
    mavlink_mission_item_int_t mav_cmd = {};
    Mission_MissionCmdToMavlinkInt(cmd, &mav_cmd);
    Blog_Mission_CMD pkt = {
        LOG_PACKET_HEADER_INIT(BLOG_MISSION_CMD_ID),
        .time_ms           = NowMs,
        .command_total     = mission->_cmd_total,
        .sequence          = mav_cmd.seq,
        .command           = mav_cmd.command,
        .param1            = mav_cmd.param1,
        .param2            = mav_cmd.param2,
        .param3            = mav_cmd.param3,
        .param4            = mav_cmd.param4,
        .latitude          = mav_cmd.x,
        .longitude         = mav_cmd.y,
        .altitude          = mav_cmd.z,
        .frame             = mav_cmd.frame,
        LOG_PACKET_END_INIT,
    };

    blog_push_msg_data(&pkt, sizeof(pkt));
}
#endif

/**
  * @brief       记录loiter相关变量
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void blog_write_loiter()
{
    uitc_mode_loiter ctrl;
    if (itc_copy_from_hub(ITC_ID(mode_loiter), &ctrl) == 0) {
        blog_loiter pkt = {
            LOG_PACKET_HEADER_INIT(BLOG_LOITER_ID),
            .time_ms           = ctrl.timestamp_us*1e-3,
            .desired_accel_x   = ctrl.desired_accel.x,
            .desired_accel_y   = ctrl.desired_accel.y,
            .predicted_accel_x = ctrl.predicted_accel.x,
            .predicted_accel_y = ctrl.predicted_accel.y,
            .brake_accel       = ctrl.brake_accel,
            .loiter_accel_brake_x = ctrl.loiter_accel_brake.x,
            .loiter_accel_brake_y = ctrl.loiter_accel_brake.y,
            .desired_vel_x = ctrl.desired_vel.x,
            .desired_vel_y = ctrl.desired_vel.y,
            .desired_speed = ctrl.desired_speed,
            .drag_decel    = ctrl.drag_decel,
            .des_vel_norm_x = ctrl.des_vel_norm.x,
            .des_vel_norm_y = ctrl.des_vel_norm.y,
            LOG_PACKET_END_INIT,
        };

        blog_push_msg_data(&pkt, sizeof(pkt));
    }
}

/*------------------------------------test------------------------------------*/


